/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received _a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "Con_ResolvedAcc.h"

namespace Robotic { namespace Mechanism{

	using namespace std;
	using namespace Robotic::Math;

	Con_ResolvedAcc::Con_ResolvedAcc(const Chain* pRobot,
		const double Kvp,
		const double Kpp,
		const double Kvo,
		const double Kpo)
		//! @brief Constructor.
	{
		_pRobot = pRobot;
		Set_Kvp(Kvp);
		Set_Kpp(Kpp);
		Set_Kvo(Kvo);
		Set_Kpo(Kpo);

	}

	void Con_ResolvedAcc::Set_Kvp(const double Kvp)
	{
		_Kvp = Kvp;
	}

	void Con_ResolvedAcc::Set_Kpp(const double Kpp)
	{
		_Kpp = Kpp;
	}

	void Con_ResolvedAcc::Set_Kvo(const double Kvo)
	{
		_Kvo = Kvo;
	}

	void Con_ResolvedAcc::Set_Kpo(const double Kpo)
	{
		_Kpo = Kpo;
	}

	ColumnVector Con_ResolvedAcc::Cmd_JointAcc(const ColumnVector & pd,
		const ColumnVector & ppd, const ColumnVector & pppd,
		const ColumnVector & wd, const ColumnVector & wpd,
		const Quaternion & quatD)
	{
		ColumnVector joint_Q = _pRobot->Get_JointValues();
		ColumnVector joint_Qp = _pRobot->Get_JointVels();
		Frame3 cart_Pose = _pRobot->ForwardKine_Pose(joint_Q);
		Velocity cart_Vel = _pRobot->ForwardKine_Vel(joint_Q,joint_Qp);

		Quaternion quatEE(cart_Pose.Rotation); // end effector orientation
		Quaternion quat;
		if(quatEE.DotProduct(quatD) < 0)
			quat = quatD*(-1);
		else
			quat = quatD;

		ColumnVector quatError = quatEE.S*quat.V - quat.S*quatEE.V + CrossProd(quatEE.V, quat.V);

		Acceleration cart_Acc;
		ColumnVector tempa = pppd + _Kvp*(ppd-cart_Vel.LinearVel) + _Kpp*(pd-cart_Pose.Translation);
		cart_Acc[0] = tempa[0];
		cart_Acc[1] = tempa[1];
		cart_Acc[2] = tempa[2];
		tempa = wpd + _Kvo*(wd-cart_Vel.AngleVel) +
			_Kpo*quatError;
		cart_Acc[3] = tempa[0];
		cart_Acc[4] = tempa[1];
		cart_Acc[5] = tempa[2];

		ColumnVector qpp(_pRobot->Get_Dof());
		_pRobot->InverseKine_Acc(joint_Q,joint_Qp,cart_Acc,qpp);

		return qpp;
	}

	ColumnVector Con_ResolvedAcc::Cmd_JointTorque(const ColumnVector & pd,
		const ColumnVector & ppd, const ColumnVector & pppd,
		const ColumnVector & wd, const ColumnVector & wpd,
		const Quaternion & quatD, const Vector3 & grav)
	{
		ColumnVector qpp = Cmd_JointAcc(pd,ppd,pppd,wd,wpd,quatD);

		Wrench wrench = Wrench(Vector3::Zero(), Vector3::Zero());
		return _pRobot->InverseDyn_GearBoxTorque(_pRobot->Get_JointValues(),_pRobot->Get_JointVels(), qpp, wrench, grav);
	}
}}